#include "BMI088.h"
#include <rtthread.h>
#include <rtdevice.h>
#include "BMI088_reg.h"
#include "RGB.h"
#include "drv_flash.h"
#include "drv_gpio.h"

#if defined (PKG_USING_SYSWATCH)
#include "syswatch.h"
#endif


#define LOG_TAG              "bmi088"
#define LOG_LVL              LOG_LVL_DBG
#include <ulog.h>

#define BMI_SPI_BUS_NAME            "spi1"
#define BMI_ACC_SPI_DEVICE_NAME     "spi10"
#define BMI_GYRO_SPI_DEVICE_NAME     "spi11"
#define IMU_TEMP_PWM_DEV_NAME        "pwm10"  
#define IMU_TEMP_PWM_CHANNEL         1   /* PWM通道 */

struct rt_device_pwm *imu_temp_pwm_dev = RT_NULL;      /* PWM设备句柄 */
struct rt_spi_device *bmi_acc_device = RT_NULL;
struct rt_spi_device *bmi_gyro_device = RT_NULL;

BMI088_Data_t BMI088_Data;
float gyroDiff[3], gNormDiff;

static void bmi088_get_accel();
static void bmi088_get_gyro();

/*底层私有函数，用于spi初始化配置和bmi088寄存器读取*/
static void _rt_hw_spi_init(void)
{
    bmi_acc_device = (struct rt_spi_device *)rt_malloc(sizeof(struct rt_spi_device));
    if(RT_NULL == bmi_acc_device)
    {
        LOG_E("Failed to malloc the spi device.");
        rt_free(bmi_acc_device);
    }
    if (RT_EOK != rt_spi_bus_attach_device_cspin(bmi_acc_device, "spi10", "spi1",GET_PIN(A,4), RT_NULL))
    {
        LOG_E("Failed to attach the spi device.");
    }

    struct rt_spi_configuration cfg;
    cfg.data_width = 8;
    cfg.mode = RT_SPI_MASTER | RT_SPI_MODE_3 | RT_SPI_MSB;
    cfg.max_hz = 10 * 1000 *1000;                           /* 10M */

    rt_spi_configure(bmi_acc_device, &cfg);

    bmi_gyro_device = (struct rt_spi_device *)rt_malloc(sizeof(struct rt_spi_device));
    if(RT_NULL == bmi_gyro_device)
    {
        LOG_E("Failed to malloc the spi device.");
        rt_free(bmi_gyro_device);
    }
    if (RT_EOK != rt_spi_bus_attach_device_cspin(bmi_gyro_device, "spi11", "spi1",GET_PIN(B,0), RT_NULL))
    {
        LOG_E("Failed to attach the spi device.");
    }

    rt_spi_configure(bmi_gyro_device, &cfg);
}
rt_inline rt_err_t _bmi088_writedata(struct rt_spi_device *device  ,rt_uint8_t addr, rt_uint8_t *data,rt_uint8_t data_len){
    rt_err_t ret=RT_EOK;
    rt_uint8_t ptdata = (addr & BMI088_SPI_WRITE_CODE);
    
    if (rt_spi_send_then_send(device,&ptdata,1,data,data_len)!=RT_EOK)
    {
        LOG_E("Failed to send data.");
        ret = RT_ERROR;
    }

    return ret;
}

rt_inline rt_err_t _bmi088_readdata(struct rt_spi_device *device  ,rt_uint8_t addr, rt_uint8_t *data,rt_uint8_t data_len){
    rt_err_t ret=RT_EOK;
    rt_uint8_t ptdata = (BMI088_SPI_READ_CODE | addr);
    rt_uint8_t tmp[8];

    RT_ASSERT(data_len <= 8)

    if (device==bmi_acc_device)
    {   
        rt_uint8_t tx[8]={0};
        tx[0]=ptdata;
        if(rt_spi_send_then_recv(device, &ptdata, 1, tmp, data_len+1)!=RT_EOK){
            LOG_E("ACC Failed to read data.");
        }else{
            rt_memcpy(data, tmp+1, data_len);//前1字节dummy byte
        }

    }
    else if(device==bmi_gyro_device){
        rt_uint8_t tx[8]={0};
        tx[0]=ptdata;
        if(rt_spi_send_then_recv(device, &ptdata, 1, tmp, data_len)!=RT_EOK){
            LOG_E("GYRO Failed to read data.");
        }else{
            rt_memcpy(data, tmp, data_len);
        }
    }

    return ret;
}

/*bmi088底层函数，用于配置bmi088和读取acc,gyro数据*/
#define BMI088REG 0
#define BMI088DATA 1
#define BMI088ERROR 2
// BMI088初始化配置数组for accel,第一列为reg地址,第二列为写入的配置值,第三列为错误码(如果出错)
static uint8_t BMI088_Accel_Init_Table[BMI088_WRITE_ACCEL_REG_NUM][3] =
    {
        {BMI088_ACC_PWR_CTRL, BMI088_ACC_ENABLE_ACC_ON, BMI088_ACC_PWR_CTRL_ERROR},
        {BMI088_ACC_PWR_CONF, BMI088_ACC_PWR_ACTIVE_MODE, BMI088_ACC_PWR_CONF_ERROR},
        {BMI088_ACC_CONF, BMI088_ACC_NORMAL | BMI088_ACC_800_HZ | BMI088_ACC_CONF_MUST_Set, BMI088_ACC_CONF_ERROR},
        {BMI088_ACC_RANGE, BMI088_ACC_RANGE_6G, BMI088_ACC_RANGE_ERROR},
        {BMI088_INT1_IO_CTRL, BMI088_ACC_INT1_IO_ENABLE | BMI088_ACC_INT1_GPIO_PP | BMI088_ACC_INT1_GPIO_LOW, BMI088_INT1_IO_CTRL_ERROR},
        {BMI088_INT_MAP_DATA, BMI088_ACC_INT1_DRDY_INTERRUPT, BMI088_INT_MAP_DATA_ERROR}};
// BMI088初始化配置数组for gyro,第一列为reg地址,第二列为写入的配置值,第三列为错误码(如果出错)
static uint8_t BMI088_Gyro_Init_Table[BMI088_WRITE_GYRO_REG_NUM][3] =
    {
        {BMI088_GYRO_RANGE, BMI088_GYRO_2000, BMI088_GYRO_RANGE_ERROR},
        {BMI088_GYRO_BANDWIDTH, BMI088_GYRO_1000_116_HZ | BMI088_GYRO_BANDWIDTH_MUST_Set, BMI088_GYRO_BANDWIDTH_ERROR},
        {BMI088_GYRO_LPM1, BMI088_GYRO_NORMAL_MODE, BMI088_GYRO_LPM1_ERROR},
        {BMI088_GYRO_CTRL, BMI088_DRDY_ON, BMI088_GYRO_CTRL_ERROR},
        {BMI088_GYRO_INT3_INT4_IO_CONF, BMI088_GYRO_INT3_GPIO_PP | BMI088_GYRO_INT3_GPIO_LOW, BMI088_GYRO_INT3_INT4_IO_CONF_ERROR},
        {BMI088_GYRO_INT3_INT4_IO_MAP, BMI088_GYRO_DRDY_IO_INT3, BMI088_GYRO_INT3_INT4_IO_MAP_ERROR}};

void VerifyAccSelfTest(void) {
    float pos_data[3], neg_data[3];
    static rt_uint8_t tmp=0;
    static uint8_t buf[6] = {0};

    tmp=BMI088_ACC_RANGE_24G;
    _bmi088_writedata(bmi_acc_device,BMI088_ACC_RANGE,&tmp,1);
    tmp=0XA7;
    _bmi088_writedata(bmi_acc_device,BMI088_ACC_CONF,&tmp,1);
    HAL_Delay(10);
    tmp=BMI088_ACC_SELF_TEST_POSITIVE_SIGNAL;
    _bmi088_writedata(bmi_acc_device,BMI088_ACC_SELF_TEST,&tmp,1);
    HAL_Delay(100);
    
    _bmi088_readdata(bmi_acc_device, BMI088_ACCEL_XOUT_L, buf, 6);
    for (uint8_t i = 0; i < 3; i++)
    {
        pos_data[i] = (BMI088_ACCEL_6G_SEN*9.80f) * (float)(int16_t)(((buf[2 * i + 1]) << 8) | buf[2 * i]);
    }

    tmp = BMI088_ACC_SELF_TEST_NEGATIVE_SIGNAL;
    _bmi088_writedata(bmi_acc_device,BMI088_ACC_SELF_TEST,&tmp,1);
    HAL_Delay(100);

    _bmi088_readdata(bmi_acc_device, BMI088_ACCEL_XOUT_L, buf, 6);
    for (uint8_t i = 0; i < 3; i++)
    {
        pos_data[i] = (BMI088_ACCEL_6G_SEN*9.80f) * (float)(int16_t)(((buf[2 * i + 1]) << 8) | buf[2 * i]);
    }

    tmp = BMI088_ACC_SELF_TEST_OFF;
    _bmi088_writedata(bmi_acc_device,BMI088_ACC_SELF_TEST,&tmp,1);
    HAL_Delay(100);
    if ((fabs(pos_data[0] - neg_data[0]) > 0.1f) || (fabs(pos_data[1] - neg_data[1]) > 0.1f) || (fabs(pos_data[2] - neg_data[2]) > 0.1f)) {
        BMI088_Data.BMI088_ERORR_CODE= BMI088_SELF_TEST_ACCEL_ERROR;
        LOG_E("Accel Self Test Failed!\n");
    }
}

void VerifyGyroSelfTest(void) {
    static rt_uint8_t tmp;

    tmp=0X01;
    _bmi088_writedata(bmi_gyro_device,BMI088_GYRO_SELF_TEST,&tmp,1);
    uint8_t bist_rdy = 0x00, bist_fail;
    while (bist_rdy == 0) {
        _bmi088_readdata(bmi_gyro_device,BMI088_GYRO_SELF_TEST,&bist_rdy,1);
        bist_rdy = (bist_rdy & 0x02) >> 1;
    }
    _bmi088_readdata(bmi_gyro_device,BMI088_GYRO_SELF_TEST,&bist_fail,1);
    bist_fail = (bist_fail & 0x04) >> 2;
    if (bist_fail == 0) {
        
    } else {
        BMI088_Data.BMI088_ERORR_CODE= BMI088_SELF_TEST_GYRO_ERROR;
    }
}

/*加速度计部分*/
void bmi088_acc_init(){
    //self check
    //id
    uint8_t whoami_check = 0;
    rt_uint8_t tmp;
    // 加速度计以I2C模式启动,需要一次上升沿来切换到SPI模式,因此进行一次fake write
    _bmi088_readdata(bmi_acc_device, BMI088_ACC_CHIP_ID, &whoami_check, 1);
    if (whoami_check != BMI088_ACC_CHIP_ID_VALUE){BMI088_Data.BMI088_ERORR_CODE= BMI088_NO_SENSOR; LOG_E("ACCEL No Sensor\n");}
    HAL_Delay(1);

    tmp=BMI088_ACC_SOFTRESET_VALUE;
    _bmi088_writedata(bmi_acc_device, BMI088_ACC_SOFTRESET, &tmp, 1); // 软复位
    HAL_Delay(BMI088_COM_WAIT_SENSOR_TIME);
        
    // 检查ID,如果不是0x1E(bmi088 whoami寄存器值),则返回错误
    // whoami_check=0;
    // _bmi088_readdata(bmi_acc_device, BMI088_ACC_CHIP_ID, &whoami_check, 1);
    // HAL_Delay(1);

    VerifyAccSelfTest();
    // 初始化寄存器,提高可读性
    uint8_t reg = 0, data = 0;
    BMI088_ERORR_CODE_e error = 0;
    // 使用sizeof而不是magic number,这样如果修改了数组大小,不用修改这里的代码;或者使用宏定义
    for (uint8_t i = 0; i < sizeof(BMI088_Accel_Init_Table) / sizeof(BMI088_Accel_Init_Table[0]); i++)
    {
        reg = BMI088_Accel_Init_Table[i][BMI088REG];
        data = BMI088_Accel_Init_Table[i][BMI088DATA];
        _bmi088_writedata(bmi_acc_device, reg, &data, 1);// 写入寄存器
        HAL_Delay(1);
        _bmi088_readdata(bmi_acc_device, reg, &data, 1);// 写完之后立刻读回检查
        HAL_Delay(1);
        if (data != BMI088_Accel_Init_Table[i][BMI088DATA])
        {    
            error |= BMI088_Accel_Init_Table[i][BMI088ERROR];
            BMI088_Data.BMI088_ERORR_CODE=error;
            LOG_E("ACCEL Init failed reg:%d\r\n",BMI088_Accel_Init_Table[i][BMI088REG]);
        }
        //{i--;} 可以设置retry次数,如果retry次数用完了,则返回error
    }
}

void bmi088_gyro_init(){
    rt_uint8_t tmp;
    
    tmp=BMI088_GYRO_SOFTRESET_VALUE;
    _bmi088_writedata(bmi_gyro_device, BMI088_GYRO_SOFTRESET, &tmp, 1);// 软复位
    rt_thread_mdelay(BMI088_COM_WAIT_SENSOR_TIME);

    // 检查ID,如果不是0x0F(bmi088 whoami寄存器值),则返回错误
    uint8_t whoami_check = 0;
    _bmi088_readdata(bmi_gyro_device, BMI088_GYRO_CHIP_ID, &whoami_check, 1);
    if (whoami_check != BMI088_GYRO_CHIP_ID_VALUE)
    {
        BMI088_Data.BMI088_ERORR_CODE= BMI088_NO_SENSOR;
        LOG_E("Gyro No Sensor\n");
    }
    rt_thread_mdelay(1);

    VerifyGyroSelfTest();

    // 初始化寄存器,提高可读性
    uint8_t reg = 0, data = 0;
    BMI088_ERORR_CODE_e error = 0;
    // 使用sizeof而不是magic number,这样如果修改了数组大小,不用修改这里的代码;或者使用宏定义
    for (uint8_t i = 0; i < sizeof(BMI088_Gyro_Init_Table) / sizeof(BMI088_Gyro_Init_Table[0]); i++)
    {
        reg = BMI088_Gyro_Init_Table[i][BMI088REG];
        data = BMI088_Gyro_Init_Table[i][BMI088DATA];
        _bmi088_writedata(bmi_gyro_device, reg, &data, 1);
        rt_thread_mdelay(1);
        _bmi088_readdata(bmi_gyro_device, reg, &data, 1);// 写完之后立刻读回对应寄存器检查是否写入成功
        rt_thread_mdelay(1);
        if (data != BMI088_Gyro_Init_Table[i][BMI088DATA])
        {
            error |= BMI088_Gyro_Init_Table[i][BMI088ERROR];
            BMI088_Data.BMI088_ERORR_CODE=error;
            LOG_E("GYRO Init failed reg:%d\r\n",BMI088_Gyro_Init_Table[i][BMI088REG]);
        }
    }
}

void bmi088_imu_temp_init(void){
    /* 查找设备 */
    imu_temp_pwm_dev = (struct rt_device_pwm *)rt_device_find(IMU_TEMP_PWM_DEV_NAME);
    if (imu_temp_pwm_dev == RT_NULL)
    {
        LOG_E("pwm init failed! can't find %s device!\n", IMU_TEMP_PWM_DEV_NAME);
    }
    
    rt_pwm_enable(imu_temp_pwm_dev,IMU_TEMP_PWM_CHANNEL);
}

void BMI088_data_acquire()
{
    static uint8_t buf[6] = {0}; // 最多读取6个byte(gyro/acc,temp是2)
    // 读取accel的x轴数据首地址,bmi088内部自增读取地址 // 3* sizeof(int16_t)
    _bmi088_readdata(bmi_acc_device, BMI088_ACCEL_XOUT_L, buf, 6);
    for (uint8_t i = 0; i < 3; i++)
        {BMI088_Data.acc[i] = (BMI088_ACCEL_6G_SEN) * (float)(int16_t)(((buf[2 * i + 1]) << 8) | buf[2 * i])* BMI088_Data.AccelScale;}
    _bmi088_readdata(bmi_gyro_device, BMI088_GYRO_X_L, buf, 6); // 连续读取3个(3*2=6)轴的角速度
    for (uint8_t i = 0; i < 3; i++)
        {BMI088_Data.gyro[i] = BMI088_GYRO_2000_SEN * (float)(int16_t)(((buf[2 * i + 1]) << 8) | buf[2 * i]) - BMI088_Data.GyroOffset[i];}
    _bmi088_readdata(bmi_acc_device, BMI088_TEMP_M, buf, 2);// 读温度,温度传感器在accel上
    int16_t tmp = (((buf[0] << 3) | (buf[1] >> 5)));
    if (tmp > 1023)
    {
        tmp-=2048;
    }
    BMI088_Data.temperature = (float)(int16_t)tmp*BMI088_TEMP_FACTOR + BMI088_TEMP_OFFSET;
}

void bmi088_get_accel()
{
    static uint8_t buf[6] = {0}; // 最多读取6个byte(gyro/acc,temp是2)
    // 读取accel的x轴数据首地址,bmi088内部自增读取地址 // 3* sizeof(int16_t)
    _bmi088_readdata(bmi_acc_device, BMI088_ACCEL_XOUT_L, buf, 6);
    for (uint8_t i = 0; i < 3; i++)
        {BMI088_Data.acc[i] = (BMI088_ACCEL_6G_SEN) * (float)((rt_int16_t)((buf[2 * i + 1]) << 8) | buf[2 * i]);}

    _bmi088_readdata(bmi_acc_device, BMI088_TEMP_M, buf, 2);// 读温度,温度传感器在accel上
    rt_int16_t tmp = (((buf[0] << 3) | (buf[1] >> 5)));
    if (tmp > 1023)
    {
        tmp-=2048;
    }
    BMI088_Data.temperature = (float)(int16_t)tmp*BMI088_TEMP_FACTOR + BMI088_TEMP_OFFSET;
}

void bmi088_get_gyro()
{
    static uint8_t buf[6] = {0}; // 最多读取6个byte(gyro/acc,temp是2)
    _bmi088_readdata(bmi_gyro_device, BMI088_GYRO_X_L, buf, 6); // 连续读取3个(3*2=6)轴的角速度
    for (uint8_t i = 0; i < 3; i++)
        {BMI088_Data.gyro[i] = BMI088_GYRO_2000_SEN * (float)((int16_t)((buf[2 * i + 1]) << 8) | buf[2 * i]);}
}

void bmi088_temp_ctrl()
{
    PIDCalculate(&BMI088_Data.imu_temp_pid, BMI088_Data.temperature, BMI088_Data.TempWhenCali);
    rt_pwm_set(imu_temp_pwm_dev,1,5000,BMI088_Data.imu_temp_pid.Output);
}

void Calibrate_MPU_Offset()
{
    static float startTime;
    static uint16_t CaliTimes = 6000; 
    float gyroMax[3], gyroMin[3];
    float gNormTemp, gNormMax, gNormMin;
    rt_uint8_t count =0;

    aRGB_led_show(LED_Yellow);
    bmi088_get_accel();

    while (1) 
    {
        bmi088_get_accel();
        PIDCalculate(&BMI088_Data.imu_temp_pid, BMI088_Data.temperature, 40);
        rt_pwm_set(imu_temp_pwm_dev,1,5000,BMI088_Data.imu_temp_pid.Output);
        if (abs(BMI088_Data.temperature-40) < 0.5f)
        {
            LOG_D("temperature set finish\n");
            break;
        }
        #if defined (PKG_USING_SYSWATCH)
        syswatch_wdt_feed();//防止校准期间被syswatch重启
        #endif
    }
    startTime = rt_tick_get();
    do
    {
        if (rt_tick_get() - startTime > 12000)
        {
            
            BMI088_Data.GyroOffset[0] = GxOFFSET;
            BMI088_Data.GyroOffset[1] = GyOFFSET;
            BMI088_Data.GyroOffset[2] = GzOFFSET;
            BMI088_Data.gNorm = gNORM;
            BMI088_Data.TempWhenCali =40;
            aRGB_led_show(LED_Red);
            LOG_E("Calibrate MPU Offset Failed!\n");
            break;
        }

        rt_thread_mdelay(5);
        BMI088_Data.gNorm = 0;
        BMI088_Data.GyroOffset[0] = 0;
        BMI088_Data.GyroOffset[1] = 0;
        BMI088_Data.GyroOffset[2] = 0;

        for (uint16_t i = 0; i < CaliTimes; i++)
        {
            #if defined (PKG_USING_SYSWATCH)
            syswatch_wdt_feed();//防止校准期间被syswatch重启
            #endif
            PIDCalculate(&BMI088_Data.imu_temp_pid, BMI088_Data.temperature, 40);
            rt_pwm_set(imu_temp_pwm_dev,1,5000,BMI088_Data.imu_temp_pid.Output);
            
            bmi088_get_accel();
            gNormTemp = sqrtf(BMI088_Data.acc[0] * BMI088_Data.acc[0] +
                              BMI088_Data.acc[1] * BMI088_Data.acc[1] +
                              BMI088_Data.acc[2] * BMI088_Data.acc[2]);
                              BMI088_Data.gNorm += gNormTemp;

            bmi088_get_gyro();
            BMI088_Data.GyroOffset[0] += BMI088_Data.gyro[0];
            BMI088_Data.GyroOffset[1] += BMI088_Data.gyro[1];
            BMI088_Data.GyroOffset[2] += BMI088_Data.gyro[2];
            if (i == 0)
            {
                gNormMax = gNormTemp;
                gNormMin = gNormTemp;
                for (uint8_t j = 0; j < 3; j++)
                {
                    gyroMax[j] = BMI088_Data.gyro[j];
                    gyroMin[j] = BMI088_Data.gyro[j];
                }
            }
            else
            {
                if (gNormTemp > gNormMax)
                    gNormMax = gNormTemp;
                if (gNormTemp < gNormMin)
                    gNormMin = gNormTemp;
                for (uint8_t j = 0; j < 3; j++)
                {
                    if (BMI088_Data.gyro[j] > gyroMax[j])
                        gyroMax[j] = BMI088_Data.gyro[j];
                    if (BMI088_Data.gyro[j] < gyroMin[j])
                        gyroMin[j] = BMI088_Data.gyro[j];
                }
            }
            gNormDiff = gNormMax - gNormMin;
            for (uint8_t j = 0; j < 3; j++)
                gyroDiff[j] = gyroMax[j] - gyroMin[j];
            if (gNormDiff > 0.5f ||
                gyroDiff[0] > 0.15f ||
                gyroDiff[1] > 0.15f ||
                gyroDiff[2] > 0.15f)
                break;
            rt_thread_mdelay(1);
        }

        BMI088_Data.gNorm /= (float)CaliTimes;
        for (uint8_t i = 0; i < 3; i++){BMI088_Data.GyroOffset[i] /= (float)CaliTimes;}

        //记录标定时的温度
        bmi088_get_accel();
        BMI088_Data.TempWhenCali =BMI088_Data.temperature;
        LOG_D("tempcali:%f\n",BMI088_Data.TempWhenCali);

    } while (gNormDiff > 0.5f ||
             fabsf(BMI088_Data.gNorm - 9.8f) > 0.5f ||
             gyroDiff[0] > 0.15f ||
             gyroDiff[1] > 0.15f ||
             gyroDiff[2] > 0.15f ||
             fabsf(BMI088_Data.GyroOffset[0]) > 0.01f ||
             fabsf(BMI088_Data.GyroOffset[1]) > 0.01f ||
             fabsf(BMI088_Data.GyroOffset[2]) > 0.01f);

    BMI088_Data.AccelScale = 9.81f / BMI088_Data.gNorm;

    rt_uint8_t tmpdata[21]={0};
    rt_memcpy(tmpdata,&BMI088_Data.GyroOffset[0],sizeof(BMI088_Data.GyroOffset));
    rt_memcpy(tmpdata+4,&BMI088_Data.GyroOffset[1],sizeof(BMI088_Data.GyroOffset));
    rt_memcpy(tmpdata+8,&BMI088_Data.GyroOffset[2],sizeof(BMI088_Data.GyroOffset));
    rt_memcpy(tmpdata+12,&BMI088_Data.gNorm,sizeof(BMI088_Data.gNorm));
    rt_memcpy(tmpdata+16,&BMI088_Data.TempWhenCali,sizeof(BMI088_Data.TempWhenCali));
    tmpdata[20]=0XAA;

    stm32_flash_erase((uint32_t)0x080E1000, sizeof(tmpdata));
    stm32_flash_write((uint32_t)0x080E1000, tmpdata, sizeof(tmpdata));

    LOG_D("calibrate MPU offset finished!\n");
}


int BMI088_init(void){
    _rt_hw_spi_init();
    /* 查找 spi 设备获取设备句柄 */
    bmi_acc_device = (struct rt_spi_device *)rt_device_find(BMI_ACC_SPI_DEVICE_NAME);
    bmi_gyro_device = (struct rt_spi_device *)rt_device_find(BMI_GYRO_SPI_DEVICE_NAME);
    if (!bmi_acc_device || !bmi_gyro_device)
    {
        LOG_E("spi init run failed! can't find  device!\n");
    }else{
        BMI088_Data.BMI088_ERORR_CODE=BMI088_NO_INIT;
        bmi088_acc_init();
        bmi088_gyro_init();     

        bmi088_imu_temp_init();

        PID_Init_Config_s config = {.MaxOut = 5000,
                                .IntegralLimit = 2000,
                                .DeadBand = 0,
                                .Kp = 2000,
                                .Ki = 200,
                                .Kd = 0,
                                .Improve = 0x01}; // enable integratiaon limit
        PIDInit(&BMI088_Data.imu_temp_pid, &config);

        rt_uint8_t tmpdata[21]={0};
        stm32_flash_read((uint32_t)0x080E1000, tmpdata, sizeof(tmpdata));
        if (tmpdata[20]!=0XAA)
        {
            Calibrate_MPU_Offset();
        }
        else 
        {
            float tmp[5] = {0.0f};
            rt_memcpy(&tmp[0], tmpdata, sizeof(tmp[0]));
            rt_memcpy(&tmp[1], tmpdata + 4, sizeof(tmp[1]));
            rt_memcpy(&tmp[2], tmpdata + 8, sizeof(tmp[2]));
            rt_memcpy(&tmp[3], tmpdata + 12, sizeof(tmp[3]));
            rt_memcpy(&tmp[4],tmpdata+16,sizeof(tmp[4]));
            

            BMI088_Data.GyroOffset[0] = tmp[0];
            BMI088_Data.GyroOffset[1] = tmp[1];
            BMI088_Data.GyroOffset[2] = tmp[2];
            BMI088_Data.gNorm         = tmp[3];
            BMI088_Data.TempWhenCali  = tmp[4];
            BMI088_Data.AccelScale    = 9.81f / BMI088_Data.gNorm;
        }

        
        if (BMI088_Data.BMI088_ERORR_CODE==BMI088_NO_INIT) {
            BMI088_Data.BMI088_ERORR_CODE=BMI088_NO_ERROR;
        }

        if (BMI088_Data.BMI088_ERORR_CODE==BMI088_NO_ERROR)
        {
            LOG_D("BMI088 init success!\n");
        }
        else 
        {
            LOG_E("BMI088 init failed! code:0x%x\n",BMI088_Data.BMI088_ERORR_CODE);
        }
    }
    return RT_EOK;
}
INIT_ENV_EXPORT(BMI088_init);